#ifndef MOTOR_CONTROL_MOTOR_CONTROLLER_H
#define MOTOR_CONTROL_MOTOR_CONTROLLER_H

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_srvs/SetBool.h>
#include"controlcan.h"
 #include <hand_drv/SetMotorPosition.h>



#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include<thread>
#include <csignal>
#include <ctime>
#include <cstdlib>
#include "unistd.h"
#include "std_msgs/Int32MultiArray.h"
class MotorController {
public:
    MotorController(ros::NodeHandle& nh);
    void start();
private:
     const int canId[8] = {0x0000,0x0601, 0x0602, 0x0603, 0x0604, 0x0605, 0x0606,0x0607}; // 硬编码的电机CAN ID

     const int START_CANOPEN[4]={0x0000,0x0000,0x0000,0x0000} ;//开启CANOPEN通信
     int start_CANOpen_cmd[2]={0x01,0x00}; //开启CANOpen 命令

    const  int  SET_MODEL[4] ={0x0301,0x0302,0x0303,0x0304}; //模式设置指令
    int set_model_data    =0x01; //位置模式

    
    const int  SET_Contrlo[4]         = {0x0201,0x0202,0x0203,0x0204}; //控制指令
    int  control_data[2]={0x00,0x00};

    const int  P_ID[4]         = {0x0501,0x0502,0x0503,0x0504};//控制指令

    int pos_data[8]={};

   const int  contral_data[2]=  {0x06,0x00};  //设置 6040h 为 06,切换至 ready to switch on 状态

    // const  int  contral_data[2]= {0x06,0x00};

     const int motorStatusID[8]={0x0000,0x0581, 0x0582, 0x0583, 0x0584, 0x0585, 0x0586,0x0587}; //状态ID



    //  int START_CAM[2]={0x01，0x00} //开启CANOPEN 通信指令


    int goal_pos[8]={0x23 ,0x7A ,0x60 ,0x00 ,0x50 ,0xC3 ,0x00 ,0x00};//目标位置
    int goal_spd[8]={0x23 ,0x81 ,0x60 ,0x00 ,0x55 ,0x55 ,0x08 ,0x00};//目标速度
     std::thread        re_loopThread;
private:
    ros::NodeHandle nh_;
    ros::Publisher position_pub_;
    ros::ServiceServer set_bool_server_;
    ros::ServiceServer set_position_server_;
    VCI_BOARD_INFO pInfo;//用来获取设备信息。
    int num=0;
    double position_;
    bool is_enabled_;


    bool handleSetBool(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
    bool handleSetMotorPosition(hand_drv::SetMotorPosition::Request &req, hand_drv::SetMotorPosition::Response &res);
    void publishPosition();
    int   init_drive(); //CAN 口初始化

    void can_recive(); //CAN口接收函数
    int send_data(int ID,  const int data[],int length);//CAN口发送函数
   //灵巧手函数
        int motor_init();//初始化电机主要是开启CAN 和电机模式
        int open_hand(); //开启手
        int close_hand();//关闭手
      int  send_data_sigle(int ID,  const int data);
      int set_posetion(int ID,int pos,int spd);

};

#endif // MOTOR_CONTROL_MOTOR_CONTROLLER_H